#include "sgl_misc.h"
#include "stdio.h"


uint32_t sgl_digits10(uint64_t number) 
{
    if (number < 10) return 1;
    if (number < 100) return 2;
    if (number < 1000) return 3;
    if (number < 1000000000000) {    // 10^12
        if (number < 100000000) {    // 10^8
            if (number < 1000000) {  // 10^6
                if (number < 10000) return 4;
                return 5 + (number >= 100000); // 10^5
            }
            return 7 + (number >= 10000000); // 10^7
        }
        if (number < 10000000000) {  // 10^10
            return 9 + (number >= 1000000000); // 10^9
        }
        return 11 + (number >= 100000000000); // 10^11
    }
    return 12 + sgl_digits10(number / 1000000000000); // 10^12
}



void sgl_int64_to_str(int64_t number, char* str_out)
{
    int text_len = 0;
    if(number > 0) {
        text_len = sgl_digits10(number);
    }
    else if(number < 0) {
        str_out[0] = '-';
        number = - number;
        text_len = sgl_digits10(number) + 1;
    }
    else {
        str_out[0] = '0';
        str_out[1] = 0;
        return ;
    }
    str_out[text_len] = 0;
    while(number) {
        text_len -- ;
        str_out[text_len] = (number%10) + 48;
        number /= 10;
    }
}

void sgl_float_to_str(double number, char* str_out)
{
    int integer = 0;
    int integer_len = 0;
    int decimal = 0;
    int decimal_len = 0;
    
    if(number < 0) {
        number = - number;
    }
    else {
    }
    integer = (int)number;
    //decimal = 
}


uint16_t sgl_fast_atan2(int x, int y)
{
    // Fast XY vector to integer degree algorithm - Jan 2011 www.RomanBlack.com
    // Converts any XY values including 0 to a degree value that should be
    // within +/- 1 degree of the accurate value without needing
    // large slow trig functions like ArcTan() or ArcCos().
    // NOTE! at least one of the X or Y values must be non-zero!
    // This is the full version, for all 4 quadrants and will generate
    // the angle in integer degrees from 0-360.
    // Any values of X and Y are usable including negative values provided
    // they are between -1456 and 1456 so the 16bit multiply does not overflow.
    unsigned char negflag;
    unsigned char tempdegree;
    unsigned char comp;
    unsigned int degree;     // this will hold the result
    //signed int x;            // these hold the XY vector at the start
    //signed int y;            // (and they will be destroyed)
    unsigned int ux;
    unsigned int uy;

    // Save the sign flags then remove signs and get XY as unsigned ints
    negflag = 0;
    if(x < 0)
    {
    negflag += 0x01;    // x flag bit
    x = (0 - x);        // is now +
    }
    ux = x;                // copy to unsigned var before multiply
    if(y < 0)
    {
    negflag += 0x02;    // y flag bit
    y = (0 - y);        // is now +
    }
    uy = y;                // copy to unsigned var before multiply

    // 1. Calc the scaled "degrees"
    if(ux > uy)
    {
    degree = (uy * 45) / ux;   // degree result will be 0-45 range
    negflag += 0x10;    // octant flag bit
    }
    else
    {
    degree = (ux * 45) / uy;   // degree result will be 0-45 range
    }

    // 2. Compensate for the 4 degree error curve
    comp = 0;
    tempdegree = degree;    // use an unsigned char for speed!
    if(tempdegree > 22)      // if top half of range
    {
    if(tempdegree <= 44) comp++;
    if(tempdegree <= 41) comp++;
    if(tempdegree <= 37) comp++;
    if(tempdegree <= 32) comp++;  // max is 4 degrees compensated
    }
    else    // else is lower half of range
    {
    if(tempdegree >= 2) comp++;
    if(tempdegree >= 6) comp++;
    if(tempdegree >= 10) comp++;
    if(tempdegree >= 15) comp++;  // max is 4 degrees compensated
    }
    degree += comp;   // degree is now accurate to +/- 1 degree!

    // Invert degree if it was X>Y octant, makes 0-45 into 90-45
    if(negflag & 0x10) degree = (90 - degree);

    // 3. Degree is now 0-90 range for this quadrant,
    // need to invert it for whichever quadrant it was in
    if(negflag & 0x02)   // if -Y
    {
    if(negflag & 0x01)   // if -Y -X
            degree = (180 + degree);
    else        // else is -Y +X
            degree = (180 - degree);
    }
    else    // else is +Y
    {
    if(negflag & 0x01)   // if +Y -X
            degree = (360 - degree);
    }
    return degree;
}

